// Copyright (c) 2010 Manuel Peinado Gallego <manuel.peinado@gmail.com>
// Distributed under the MIT license


#include "stdafx.h"
#include "WorldBuilder.h"
#include "Seg.h"

namespace
{
    double dist2_pnt_to_seg(const gx::Vec3 & point, const Seg & seg, 
                            gx::Vec3 & closestPnt);
    double dist2_seg_to_seg(Seg const & seg0, Seg const & seg1, 
                        std::pair<gx::Vec3, gx::Vec3> & closestPnts);
}

double Seg::dist2(Seg const & other, 
                  std::pair<gx::Vec3, gx::Vec3> & closestPnts) const
{
    return dist2_seg_to_seg(*this, other, closestPnts);
}

gx::Vec3 Seg::closestPnt(const gx::Vec3 & point) const
{   
    gx::Vec3 closestOnSeg;
    dist2_pnt_to_seg(point, *this, closestOnSeg);
    return closestOnSeg;
}

namespace
{

double dist2_pnt_to_seg(const gx::Vec3 & point, const Seg & seg,
                        gx::Vec3 & closestPnt)
{
    gx::Vec3 const diff = point - seg.origin;
    double t = seg.dir.dot(diff);
    double ext = t < -seg.ext ? -seg.ext : (t > seg.ext ? seg.ext : t);
    closestPnt = seg.origin + seg.dir * ext;
    return point.dist2(closestPnt);
}


double dist2_seg_to_seg(Seg const & seg0, Seg const & seg1, 
                        std::pair<gx::Vec3, gx::Vec3> & closestPnts)
{
    double dist2;

    gx::Vec3 const diff = seg0.origin - seg1.origin;
    double a01 = -seg0.dir.dot(seg1.dir);
    double b0 = diff.dot(seg0.dir);
    double b1 = -diff.dot(seg1.dir);
    double c = diff.norm2();
    double det = fabs(1.0 - a01 * a01);

    if(det >= 1e-8)  // segments are not parallel
    {
        double s0 = a01 * b1-b0;
        double s1 = a01 * b0-b1;
        double ext_det0 = seg0.ext * det;
        double ext_det1 = seg1.ext * det;

        if(s0 >= -ext_det0)
        {
            if(s0 <= ext_det0)
            {
                if(s1 >= -ext_det1)
                {
                    if(s1 <= ext_det1)  // region 0 (interior)
                    {
                        // minimum at two interior points of 3D lines
                        double fInvDet = 1 / det;
                        s0 *= fInvDet;
                        s1 *= fInvDet;
                        dist2 = s0*(s0+a01*s1+2.0*b0) + s1*(a01*s0+s1+2.0*b1)+c;
                    }
                    else  // region 3 (side)
                    {
                        s1 = seg1.ext;
                        double fTmpS0 = -(a01*s1+b0);
                        if(fTmpS0 < -seg0.ext)
                        {
                            s0 = -seg0.ext;
                            dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                        }
                        else if(fTmpS0 <= seg0.ext)
                        {
                            s0 = fTmpS0;
                            dist2 = -s0*s0+s1*(s1+2.0*b1)+c;
                        }
                        else
                        {
                            s0 = seg0.ext;
                            dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                        }
                    }
                }
                else  // region 7 (side)
                {
                    s1 = -seg1.ext;
                    double fTmpS0 = -(a01*s1+b0);
                    if(fTmpS0 < -seg0.ext)
                    {
                        s0 = -seg0.ext;
                        dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                    }
                    else if(fTmpS0 <= seg0.ext)
                    {
                        s0 = fTmpS0;
                        dist2 = -s0*s0+s1*(s1+2.0*b1)+c;
                    }
                    else
                    {
                        s0 = seg0.ext;
                        dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                    }
                }
            }
            else
            {
                if(s1 >= -ext_det1)
                {
                    if(s1 <= ext_det1)  // region 1 (side)
                    {
                        s0 = seg0.ext;
                        double fTmpS1 = -(a01*s0+b1);
                        if(fTmpS1 < -seg1.ext)
                        {
                            s1 = -seg1.ext;
                            dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                        }
                        else if(fTmpS1 <= seg1.ext)
                        {
                            s1 = fTmpS1;
                            dist2 = -s1*s1+s0*(s0+2.0*b0)+c;
                        }
                        else
                        {
                            s1 = seg1.ext;
                            dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                        }
                    }
                    else  // region 2 (corner)
                    {
                        s1 = seg1.ext;
                        double fTmpS0 = -(a01*s1+b0);
                        if(fTmpS0 < -seg0.ext)
                        {
                            s0 = -seg0.ext;
                            dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                        }
                        else if(fTmpS0 <= seg0.ext)
                        {
                            s0 = fTmpS0;
                            dist2 = -s0*s0+s1*(s1+2.0*b1)+c;
                        }
                        else
                        {
                            s0 = seg0.ext;
                            double fTmpS1 = -(a01*s0+b1);
                            if(fTmpS1 < -seg1.ext)
                            {
                                s1 = -seg1.ext;
                                dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                            }
                            else if(fTmpS1 <= seg1.ext)
                            {
                                s1 = fTmpS1;
                                dist2 = -s1*s1+s0*(s0+2.0*b0) + c;
                            }
                            else
                            {
                                s1 = seg1.ext;
                                dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                            }
                        }
                    }
                }
                else  // region 8 (corner)
                {
                    s1 = -seg1.ext;
                    double fTmpS0 = -(a01*s1+b0);
                    if(fTmpS0 < -seg0.ext)
                    {
                        s0 = -seg0.ext;
                        dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                    }
                    else if(fTmpS0 <= seg0.ext)
                    {
                        s0 = fTmpS0;
                        dist2 = -s0*s0+s1*(s1+2.0*b1)+c;
                    }
                    else
                    {
                        s0 = seg0.ext;
                        double fTmpS1 = -(a01*s0+b1);
                        if(fTmpS1 > seg1.ext)
                        {
                            s1 = seg1.ext;
                            dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                        }
                        else if(fTmpS1 >= -seg1.ext)
                        {
                            s1 = fTmpS1;
                            dist2 = -s1*s1+s0*(s0+2.0*b0) + c;
                        }
                        else
                        {
                            s1 = -seg1.ext;
                            dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                        }
                    }
                }
            }
        }
        else 
        {
            if(s1 >= -ext_det1)
            {
                if(s1 <= ext_det1)  // region 5 (side)
                {
                    s0 = -seg0.ext;
                    double fTmpS1 = -(a01*s0+b1);
                    if(fTmpS1 < -seg1.ext)
                    {
                        s1 = -seg1.ext;
                        dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                    }
                    else if(fTmpS1 <= seg1.ext)
                    {
                        s1 = fTmpS1;
                        dist2 = -s1*s1+s0*(s0+2.0*b0)+c;
                    }
                    else
                    {
                        s1 = seg1.ext;
                        dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                    }
                }
                else  // region 4 (corner)
                {
                    s1 = seg1.ext;
                    double fTmpS0 = -(a01*s1+b0);
                    if(fTmpS0 > seg0.ext)
                    {
                        s0 = seg0.ext;
                        dist2 = s0*(s0-2.0*fTmpS0) + s1*(s1+2.0*b1)+c;
                    }
                    else if(fTmpS0 >= -seg0.ext)
                    {
                        s0 = fTmpS0;
                        dist2 = -s0*s0+s1*(s1+2.0*b1)+c;
                    }
                    else
                    {
                        s0 = -seg0.ext;
                        double fTmpS1 = -(a01*s0+b1);
                        if(fTmpS1 < -seg1.ext)
                        {
                            s1 = -seg1.ext;
                            dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                        }
                        else if(fTmpS1 <= seg1.ext)
                        {
                            s1 = fTmpS1;
                            dist2 = -s1*s1+s0*(s0+2.0*b0) + c;
                        }
                        else
                        {
                            s1 = seg1.ext;
                            dist2 = s1*(s1-2.0*fTmpS1) + s0*(s0+2.0*b0)+c;
                        }
                    }
                }
            }
            else   // region 6 (corner)
            {
                s1 = -seg1.ext;
                double fTmpS0 = -(a01*s1+b0);
                if(fTmpS0 > seg0.ext)
                {
                    s0 = seg0.ext;
                    dist2 = s0*(s0-2.0*fTmpS0) +
                        s1*(s1+2.0*b1)+c;
                }
                else if(fTmpS0 >= -seg0.ext)
                {
                    s0 = fTmpS0;
                    dist2 = -s0*s0+s1*(s1+2.0*b1)+c;
                }
                else
                {
                    s0 = -seg0.ext;
                    double fTmpS1 = -(a01*s0+b1);
                    if(fTmpS1 < -seg1.ext)
                    {
                        s1 = -seg1.ext;
                        dist2 = s1*(s1-2.0*fTmpS1) +
                            s0*(s0+2.0*b0)+c;
                    }
                    else if(fTmpS1 <= seg1.ext)
                    {
                        s1 = fTmpS1;
                        dist2 = -s1*s1+s0*(s0+2.0*b0)
                            + c;
                    }
                    else
                    {
                        s1 = seg1.ext;
                        dist2 = s1*(s1-2.0*fTmpS1) +
                            s0*(s0+2.0*b0)+c;
                    }
                }
            }
        }

        closestPnts = std::make_pair(seg0.origin + s0 * seg0.dir, 
                                     seg1.origin + s1 * seg1.dir);
        dist2 = fabs(dist2);
    }
    else
    {
        // Project P1_1 onto S0
        gx::Vec3 proj_on_s0 = seg0.closestPnt(seg1.p0());
        closestPnts = std::make_pair(proj_on_s0, seg1.closestPnt(proj_on_s0));
        dist2 = closestPnts.first.dist2(closestPnts.second);
    }
    return dist2;
}

}